#pragma once

#include "i2c_v1.hpp"
#include "yj_return.hpp"
#include <glog/logging.h>
#include <thread>
#include <chrono>
#include <math.h>


//使用前需要使用上位机执行自动校准
class hwt101
{
public:
    typedef enum
    {
        save_reg_addr = 0x00,
        rrate_reg_addr = 0x03,
        version_reg_addr = 0x2E,
        yaw_reg_addr = 0x3F,
        work_mode_reg_addr = 0x48,
        cali_yaw_reg_addr = 0x76,
        no_auto_cali_reg_addr = 0xA7,
    } reg_addr;

private:
    std::string _dev_name;
    i2c_v1 *_i2c;
    uint8_t _device_address;
    double _last_yaw;
    double _yaw_offset;
    yj_return _flag;

    yj_return _read_reg(reg_addr reg, uint16_t *data, uint8_t len);
    yj_return _write_reg(reg_addr reg, uint16_t *data, uint8_t len);

public:
    hwt101(std::string dev_name, i2c_v1 *i2c, uint8_t device_address);
    ~hwt101();

    yj_return init();
    yj_return reset();

    yj_return read_yaw(double *yaw);
    yj_return read_yaw_for_pid(double *yaw);

    yj_return read_single_reg(reg_addr reg, uint16_t *reg_out);
    yj_return write_single_reg(reg_addr reg, uint16_t reg_out);

    yj_return read_status();
    void print_config();
};